'''
Created on Sep 29, 2010

@author: diego

'''
import cv
import pyopencv as pycv
import numpy


def getHomography(srcKeypoints,dstKeypoints):
    dim=len(srcKeypoints)
    srcMat=cv.CreateMat(dim,2,cv.CV_32FC1)
    dstMat=cv.CreateMat(dim,2,cv.CV_32FC1)
    for i in range(1,dim):
        srcMat[i,0]=srcKeypoints[i][0]
        srcMat[i,1]=srcKeypoints[i][1]
        dstMat[i,0]=dstKeypoints[i][0]
        dstMat[i,1]=dstKeypoints[i][1]
    
    h=cv.CreateMat(3,3,cv.CV_64F)
    cv.FindHomography(srcMat,dstMat, h,cv.CV_RANSAC,5)
    
#    for i in range(3):
#        print str(h[i,0])+','+str(h[i,1])+','+str(h[i,2])
    
    return h


def drawPoints(image,points,size=10):
    for (x,y) in points:
        cv.Circle(image, (x,y), size, (200,50,50))
    
def drawKeypoints(image,srcKeyp):
    for ((x, y), laplacian, size, dir, hessian) in srcKeyp:
        cv.Circle(image, (x,y), size, (200,50,50))
        
def findPairsNaive(srcSURF,dstSURF,maxFeatures=100):
    i=0
    Nearest_Neighbor_Threshold = 1.0
    (srcKeyp,srcDesc)=srcSURF
    (dstKeyp,dstDesc)=dstSURF
    pairs=[]
    for d in srcDesc:
        j=0
        for e in dstDesc:
            if srcKeyp[i][1] == dstKeyp[j][1] and compare_desc(d, e)<Nearest_Neighbor_Threshold:
                pairs.append((srcKeyp[i][0],dstKeyp[j][0]))
#                drawKeypoints(match, [srcKeyp[i]])
#                drawKeypoints(match2, [dstKeyp[j]])
                if(len(pairs)>maxFeatures):
                    return pairs
        

            j=j+1
        i=i+1
    return pairs

def buildSearchKDTree(modelSurf):
    (_,modelDesc)=modelSurf
    
    
    modelFeatureMatrix=pycv.asMat(numpy.array(modelDesc,dtype='float32'))
    flann = pycv.Index(modelFeatureMatrix,pycv.KDTreeIndexParams(4))
    
    return flann


def findPairsFlann(modelSurf,dstSurf,flann):
    
    ptpairs=[]
    

    (modelKeyp,modelDesc)=modelSurf
    modelLen=len(modelDesc)
#    if(flann==None):
    modelFeatureMatrix=pycv.asMat(numpy.array(modelDesc,dtype='float32'))
    flann = pycv.Index(modelFeatureMatrix,pycv.KMeansIndexParams())

    
    (dstKeyp,dstDesc)=dstSurf
    searchLen=len(dstDesc)
    
    searchFeaturesMatrix=pycv.asMat(numpy.array(dstDesc,dtype='float32'))
    
    indices = pycv.Mat(searchLen, modelLen, cv.CV_32S)
    dists = pycv.Mat(searchLen, modelLen, cv.CV_32F)
    
    flann.knnSearch(searchFeaturesMatrix,indices,dists,1,pycv.SearchParams(250))
    
    
    indices = indices[:,0]
#    dists = dists.ndarray
    
    for i in xrange(searchLen):
        if dists[i,0] < 0.6*dists[i,1] and dists[i,0]<0.5:
            ptpairs.append((modelKeyp[indices[i]][0],dstKeyp[i][0]))
    
    return ptpairs


#def findPairsNotSoNaive(srcSURF,dstSURF,maxFeatures=20):
#    i=0
#    srcSURFAcum=dict()
#    Nearest_Neighbor_Threshold = 1.0
#    (modelKeyp,modelDesc)=srcSURF
#    (dstKeyp,dstDesc)=dstSURF
#    pairs=[]
#    for d in modelDesc:
#        j=0
#        for e in dstDesc:
#            if modelKeyp[i][1] == dstKeyp[j][1] and compare_desc(d, e)<Nearest_Neighbor_Threshold:
#                pairs.append((modelKeyp[i][0],dstKeyp[j][0]))
#)
#                if(len(pairs)>maxFeatures):
#                    return pairs
#        
#
#            j=j+1
#        i=i+1
#    return pairs
    


def compare_desc(desc1,dstDesc):
    a=zip(desc1,dstDesc)
    sum=0
    for (x,y) in a:
        sum=sum + abs(x-y)
    return sum

#def compare_one_to_N_desc(desc,modelDescs):
#    acum=dict()
#    for i in range(len(modelDescs)
#        for modelDesc in modelDescs:
#            acum[i]+=abs(modelDescs[i]-desc[i])
#    
#    return acum.index(min(acum))    